#region License
// Copyright 2007-2008 Rory Plaire (codekaizen@gmail.com)
/*
 * This license governs use of the accompanying software. If you use the software, you
 * accept this license. If you do not accept the license, do not use the software.
 * 
 * 1. Definitions
 * The terms "reproduce," "reproduction," "derivative works," and "distribution" have the
 * same meaning here as under U.S. copyright law.
 * A "contribution" is the original software, or any additions or changes to the software.
 * A "contributor" is any person that distributes its contribution under this license.
 * "Licensed patents" are a contributor's patent claims that read directly on its contribution.
 * 
 * 2. Grant of Rights
 * (A) Copyright Grant- Subject to the terms of this license, including the license conditions 
 *     and limitations in section 3, each contributor grants you a non-exclusive, worldwide, 
 *     royalty-free copyright license to reproduce its contribution, prepare derivative works 
 *     of its contribution, and distribute its contribution or any derivative works that you create.
 * (B) Patent Grant- Subject to the terms of this license, including the license conditions and 
 *     limitations in section 3, each contributor grants you a non-exclusive, worldwide, royalty-free 
 *     license under its licensed patents to make, have made, use, sell, offer for sale, import, 
 *     and/or otherwise dispose of its contribution in the software or derivative works of the 
 *     contribution in the software.
 * 
 * 3. Conditions and Limitations
 * (A) No Trademark License- This license does not grant you rights to use any contributors' 
 *     name, logo, or trademarks.
 * (B) If you bring a patent claim against any contributor over patents that you claim are 
 *     infringed by the software, your patent license from such contributor to the software 
 *     ends automatically.
 * (C) If you distribute any portion of the software, you must retain all copyright, patent, 
 *     trademark, and attribution notices that are present in the software.
 * (D) If you distribute any portion of the software in source code form, you may do so only under 
 *     this license by including a complete copy of this license with your distribution. If you 
 *     distribute any portion of the software in compiled or object code form, you may only do so 
 *     under a license that complies with this license.
 * (E) The software is licensed "as-is." You bear the risk of using it. The contributors give no 
 *     express warranties, guarantees or conditions. You may have additional consumer rights under 
 *     your local laws which this license cannot change. To the extent permitted under your local laws, 
 *     the contributors exclude the implied warranties of merchantability, fitness for a particular 
 *     purpose and non-infringement.
 * 
 */
#endregion

// Based initially on JAMA : A Java Matrix Package by 
// National Institute of Standards and Technology (NIST) 
// and The MathWorks, a public-domain work.

using System;
using System.Diagnostics;
using NPack.Numeric;
using NPack.Numeric.Interfaces;

namespace NPack
{
    /// <summary>
    ///	QR decomposition for a rectangular matrix.
    /// </summary>
    /// <remarks>
    /// <para>
    /// For an m-by-n Matrix{T} <c>A</c> with <c>m &gt;= n</c>, the QR decomposition is an m-by-n
    /// orthogonal Matrix{T} <c>Q</c> and an n-by-n upper triangular 
    /// Matrix{T} <c>R</c> so that <c>A = Q * R</c>.
    /// </para>
    /// <para>
    /// The QR decompostion always exists, even if the Matrix{T} does not have
    /// full rank, so the constructor will never fail.  The primary use of the
    /// QR decomposition is in the least squares solution of non-square systems
    /// of simultaneous linear equations. It is well-suited to computation due to the
    /// fact that the Q matrix in the decomposition is an orthogonal matrix, allowing
    /// errors that arise in its computation to be removed by reforming it to orthogonality.
    /// </para>
    /// </remarks>
    public class HouseholderReflectionQrDecomposition<TComponent, TMatrix> : IQRDecomposition<TComponent, TMatrix>
        //where TComponent : IEquatable<TComponent>, IComparable<TComponent>, IComputable<TComponent>, IConvertible, IFormattable
        where TMatrix : IMatrix<TComponent, TMatrix>, IEquatable<TMatrix>, IComparable<TMatrix>, IComputable<TMatrix>
    {
        private readonly TMatrix _inputMatrix;
        private readonly TMatrix _qr;
        private readonly TComponent[] _rdiag;
        private static readonly Real<TComponent> _zero = default(Real<TComponent>);
        private static readonly Real<TComponent> _one = default(Real<TComponent>).One;
        private readonly IMatrixFactory<TComponent, TMatrix> _factory;

        /// <summary>
        /// Construct a QR decomposition.
        /// </summary>	
        public HouseholderReflectionQrDecomposition(TMatrix matrix, IMatrixFactory<TComponent, TMatrix> factory)
        {
            if (factory == null)
            {
                throw new ArgumentNullException("factory");
            }

            if (Equals(matrix, default(TMatrix)))
            {
                throw new ArgumentNullException("matrix");
            }

            _factory = factory;

            Int32 m = matrix.RowCount;
            Int32 n = matrix.ColumnCount;
            _inputMatrix = (TMatrix)matrix.Clone();
            _qr = (TMatrix)matrix.Clone();
            _rdiag = new TComponent[n];

            for (Int32 k = 0; k < n; k++)
            {
                // Compute 2-norm of k-th column without under/overflow.
                TComponent norm = _zero;

                for (Int32 i = k; i < m; i++)
                {
                    norm = new Real<TComponent>(Utility.Hypotenuse(norm, _qr[i, k]));
                }

                if (!norm.Equals(_zero))
                {
                    // Form k-th Householder vector.
                    if ((Real<TComponent>)_qr[k, k] < (Real<TComponent>)default(TComponent))
                    {
                        norm = ((Real<TComponent>)norm).Negative();
                    }

                    for (Int32 i = k; i < m; i++)
                    {
                        _qr[i, k] = (Real<TComponent>)_qr[i, k] / norm;
                    }

                    _qr[k, k] = (Real<TComponent>)_qr[k, k] + _one;

                    // Apply transformation to remaining columns.
                    for (Int32 j = k + 1; j < n; j++)
                    {
                        TComponent s = _zero;

                        for (Int32 i = k; i < m; i++)
                        {
                            s = s + ((Real<TComponent>)_qr[i, k] * _qr[i, j]);
                        }

                        s = ((Real<TComponent>)((Real<TComponent>)s).Negative()) / _qr[k, k];

                        for (Int32 i = k; i < m; i++)
                        {
                            _qr[i, j] = _qr[i, j] + ((Real<TComponent>)s * _qr[i, k]);
                        }
                    }
                }

                _rdiag[k] = ((Real<TComponent>)norm).Negative();
            }
        }

        public TMatrix HouseholderVectors
        {
            get
            {
                TMatrix x = _factory.CreateMatrix(MatrixFormat.ColumnMajor, _qr.RowCount, _qr.ColumnCount);

                for (Int32 i = 0; i < x.RowCount; i++)
                {
                    for (Int32 j = 0; j < x.ColumnCount; j++)
                    {
                        if (i >= j)
                        {
                            x[i, j] = _qr[i, j];
                        }
                        else
                        {
                            x[i, j] = _zero;
                        }
                    }
                }

                return x;
            }
        }

        #region IQrDecomposition<T> Members

        /// <summary>
        /// Gets a value which indicates if the Matrix{T} <c>A</c> is of full rank.
        /// </summary>
        /// <value>
        /// <see langword="true"/> if <c>R</c>, and hence <c>A</c>, has full rank; 
        /// <see langword="false"/> otherwise.
        /// </value>
        public Boolean IsFullRank
        {
            get
            {
                Int32 columns = _qr.ColumnCount;

                for (Int32 i = 0; i < columns; i++)
                {
                    if (_rdiag[i].Equals(0))
                    {
                        return false;
                    }
                }

                return true;
            }
        }

        /// <summary>
        /// Returns the orthogonal factor <c>Q</c>.
        /// </summary>
        public TMatrix Q
        {
            get
            {
                TMatrix q = _factory.CreateMatrix(MatrixFormat.ColumnMajor, _qr.RowCount, _qr.ColumnCount);

                TComponent one = default(Real<TComponent>).One;

                for (Int32 k = _qr.ColumnCount - 1; k >= 0; k--)
                {
                    for (Int32 i = 0; i < _qr.RowCount; i++)
                    {
                        q[i, k] = default(TComponent);
                    }

                    q[k, k] = one;

                    for (Int32 j = k; j < _qr.ColumnCount; j++)
                    {
                        if (!_qr[k, k].Equals(default(TComponent)))
                        {
                            TComponent s = default(TComponent);

                            for (Int32 i = k; i < _qr.RowCount; i++)
                            {
                                s = s + ((Real<TComponent>)_qr[i, k] * q[i, j]);
                            }

                            s = ((Real<TComponent>)((Real<TComponent>)s).Negative()) / _qr[k, k];

                            for (Int32 i = k; i < _qr.RowCount; i++)
                            {
                                q[i, j] = q[i, j] + ((Real<TComponent>)s * _qr[i, k]);
                            }
                        }
                    }
                }

                return q;
            }
        }

        /// <summary>
        /// Returns the upper triangular factor <c>R</c>.
        /// </summary>
        public TMatrix R
        {
            get
            {
                Int32 n = _qr.ColumnCount;
                TMatrix x = _factory.CreateMatrix(MatrixFormat.ColumnMajor, n, n);

                for (Int32 i = 0; i < n; i++)
                {
                    for (Int32 j = 0; j < n; j++)
                    {
                        if (i < j)
                        {
                            x[i, j] = _qr[i, j];
                        }
                        else if (i == j)
                        {
                            x[i, j] = _rdiag[i];
                        }
                        else
                        {
                            x[i, j] = _zero;
                        }
                    }
                }

                return x;
            }
        }

        /// <summary>
        /// Least squares solution of <c>A * X = B</c>.
        /// </summary>
        /// <param name="value">
        /// Right-hand-side Matrix{T} with as many rows as <c>A</c> and any number of columns.
        /// </param>
        /// <returns>
        /// A Matrix{T} that minimized the two norm of <c>Q * R * X - B</c>.
        /// </returns>
        /// <exception cref="T:System.ArgumentException">
        /// Matrix{T} row dimensions must be the same.
        /// </exception>
        /// <exception cref="System.InvalidOperationException">
        /// Matrix{T} is rank deficient.
        /// </exception>
        public TMatrix Solve(TMatrix value)
        {
            if (Equals(value, default(TMatrix)))
            {
                throw new ArgumentNullException("value");
            }

            if (value.RowCount != _qr.RowCount)
            {
                throw new ArgumentException("Matrix<T> row dimensions must agree.");
            }

            if (!IsFullRank)
            {
                throw new InvalidOperationException("Matrix<T> is rank deficient.");
            }

            // Copy right hand side
            Int32 count = value.ColumnCount;
            TMatrix x = value.Clone();
            Debug.Assert(x != null);
            Int32 m = _qr.RowCount;
            Int32 n = _qr.ColumnCount;

            // Compute Y = transpose(Q)*B
            for (Int32 k = 0; k < n; k++)
            {
                for (Int32 j = 0; j < count; j++)
                {
                    TComponent s = _zero;

                    for (Int32 i = k; i < m; i++)
                    {
                        s = s + ((Real<TComponent>)_qr[i, k] * x[i, j]);
                    }

                    s = (((Real<TComponent>)s / _qr[k, k])).Negative();

                    for (Int32 i = k; i < m; i++)
                    {
                        x[i, j] = x[i, j] + ((Real<TComponent>)s * _qr[i, k]);
                    }
                }
            }

            // Solve R*X = Y;
            for (Int32 k = n - 1; k >= 0; k--)
            {
                for (Int32 j = 0; j < count; j++)
                {
                    x[k, j] = (Real<TComponent>)x[k, j]/ _rdiag[k];
                }

                for (Int32 i = 0; i < k; i++)
                {
                    for (Int32 j = 0; j < count; j++)
                    {
                        x[i, j] = ((Real<TComponent>)x[i, j] - x[k, j]) * _qr[i, k];
                    }
                }
            }

            return x.GetMatrix(0, n - 1, 0, count - 1);
        }

        #endregion

        #region IFactorization<T> Members

        public TMatrix InputMatrix
        {
            get { return (TMatrix)_inputMatrix.Clone(); }
        }

        #endregion
    }
}